import sensor
import time
from machine import UART
from machine import LED
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA)   # QVGA:320x240 - QQVGA:160x120
sensor.set_auto_exposure(1)                                 # 设置自动曝光
sensor.set_auto_gain(0, gain_db = 17)                       # 设置画面增益 17 dB 影响实时画面亮度
sensor.set_auto_whitebal(0, rgb_gain_db = (0,0,0))          # 设置RGB增益 0 0 0 dB 影响画面色彩呈现效果 在 K210 上无法调节增益 初步判定是感光元件 ov2640 无法支持
sensor.skip_frames(time = 2000)
clock = time.clock()
uart = UART(3, 9600)              # UART(3)P4-TX P5-RX
uart.init(9600,bits=8,parity=None,stop=1)
led = LED("LED_GREEN")
led.on()
#*********************************************
red_thresholds = [(81, 100, 2, 127, -128, 127)]
green_thresholds = [(81, 100, -41, -15, -128, 127)]
roi_ = [23,11,121,86]
rect_ = 0
red_point_ = [0,0]
green_point_ = [0,0]
#*********************************************

# 计算两点距离
def point_distance(a,b):
    return (abs(a[0]-b[0])**2 + abs(a[1]-b[1])**2)**0.5

# 找矩形
def find_rect(img):
    global rect_
    magnitude = 0
    for r in img.find_rects(roi=roi_,threshold=50000):
        # 查找最大面积值
        if r.magnitude() > magnitude:
            rect_ = r
    # 更新ROI
    if rect_ != 0:
        dis = 20
        if rect_[0] - dis - 5 > 0:
            roi_[0] = rect_[0] - dis - 5
        else :
            roi_[0] = 0
        if rect_[1] - dis > 0:
            roi_[1] = rect_[1] - dis
        else :
            roi_[1] = 0
        if roi_[0] + rect_[2] + dis * 2 + 2 * 5 < 160:
            roi_[2] = rect_[2] + dis * 2 + 2 * 5
        else :
            roi_[2] = 160 - roi_[0]
        if roi_[1] + rect_[3] + dis * 2 < 120:
            roi_[3] = rect_[3] + dis * 2
        else :
            roi_[3] = 120 - roi_[1]

# 找光点:根据颜色全局搜索，找不到在从方框中逐个像素点查找
red_point = [0,0]
green_point = [0,0]
def find_point(img,ROI):
    global red_point,green_point
    # point_ = red_point = green_point = [0,0]      # !!!!!此处会将red_point和green_point指向同一块内存
    point_ =  [0,0]
    red_point =  [0,0]
    green_point = [0,0]
    for x in range(ROI[0],ROI[0]+ROI[2]):
        for y in range(ROI[1],ROI[1]+ROI[3]):
            if point_distance([x,y],point_) > 10:
                rgb = img.get_pixel(x,y)
                if sum(rgb) > 230*3:
                    for x1 in range(1,10):
                        rgb = img.get_pixel(x+x1,y)
                        if sum(rgb) < 230*3:
                            point_[0] = x + x1 // 2
                            point_[1] = y
                            # 根据x轴长度阵列扫描比较RG值
                            R = G = 0
                            for x2 in range(point_[0]-(x1 // 2 + 2),point_[0]+(x1 // 2 + 2) + 1):
                                for y2 in range(point_[1]-2,point_[1]+2+1):
                                    rgb = img.get_pixel(x2,y2)
                                    if rgb[0] > rgb[1]:     # R > G
                                        R += 1
                                    else :
                                        G += 1
                            if R > G:
                                red_point = point_.copy()   # 这里必须使用copy()!
                            else :
                                green_point = point_.copy()
                            if red_point[0] != 0 and red_point[1] != 0 and green_point[0] != 0 and green_point[1] != 0:
                                return
                            break

# 画矩形画圆点
def draw(img):
    global rect_,red_point,green_point
    # 画矩形
    if rect_ != 0:
        img.draw_rectangle(rect_.rect(), color = (255, 0, 0), thickness=1)
        for p in rect_.corners():
            img.draw_string (p[0], p[1], str(p[0])+','+str(p[1]), color=(255, 255, 255),mono_space=False)
    # 画圆点
    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if red_point[0] != 0 and red_point[1] != 0:
        img.draw_circle(red_point[0], red_point[1], 5, thickness=2, color = (255, 0, 0))
        img.draw_string (red_point[0], red_point[1]+10, str(red_point[0])+','+str(red_point[1]), color=(255, 255, 255),mono_space=False)
    img.draw_rectangle(tuple(roi_), color = (255, 0, 0), thickness=1)
    if green_point[0] != 0 and green_point[1] != 0:
        img.draw_circle(green_point[0], green_point[1], 5, thickness=2, color = (0, 255, 0))
        img.draw_string (green_point[0], green_point[1]+10, str(green_point[0])+','+str(green_point[1]), color=(255, 255, 255),mono_space=False)

def Usart_send():
    global red_point,green_point
    if red_point[0] != 0 and red_point[1] != 0 and green_point[0] != 0 and green_point[1] != 0:
        data = [0xa0,0xb0]
        data.append(red_point[0])
        data.append(red_point[1])
        data.append(green_point[0])
        data.append(green_point[1])
        data.append(0xc0)
        uart.write(bytearray(data))
        print(data)

while True :
    clock.tick()
    img = sensor.snapshot()

    find_rect(img)              # 矩形检测
    find_point(img,roi_)        # 光点检测
    Usart_send()                # 发送数据
    draw(img)                   # 画框画点

    fps = clock.fps()
    img.draw_string(0, 0, "%2.1ffps" %(fps), color=(0, 0, 255), scale=1.0)
    print("fps:" + str(fps))


